
#include <cstdint>
#include <memory>
#include <string>

#include <OgreSceneManager.h>
#include <OgreSceneNode.h>

#include <rviz_common/display_context.hpp>
#include <rviz_common/frame_manager_iface.hpp>
#include <rviz_common/properties/parse_color.hpp>
#include <rviz_common/properties/property.hpp>
#include <rviz_common/interaction/selection_manager.hpp>
#include <rviz_rendering/objects/grid.hpp>

#include "rviz_custom_plugins/displays/custom_grid_display.h"

namespace rviz_custom_plugins
{
namespace displays
{

using rviz_common::properties::ColorProperty;
using rviz_common::properties::EnumProperty;
using rviz_common::properties::FloatProperty;
using rviz_common::properties::IntProperty;
using rviz_common::properties::qtToOgre;
using rviz_common::properties::StatusProperty;
using rviz_common::properties::TfFrameProperty;
using rviz_common::properties::VectorProperty;
using rviz_rendering::Grid;

CustomGridDisplay::CustomGridDisplay()
{
  frame_property_ = new TfFrameProperty(
    "Reference Frame", TfFrameProperty::FIXED_FRAME_STRING,
    "The TF frame this grid will use for its origin.",
    this, 0, true);
  frame_property_->hide();

  cell_size_property_ = new FloatProperty(
    "Cell Size", 1.0f,
    "The length, in meters, of the side of each cell.",
    this, SLOT(updateCellSize()));
  cell_size_property_->setMin(0.0001f);

  color_property_ = new ColorProperty(
    "Color", Qt::gray,
    "The color of the grid lines.",
    this, SLOT(updateColor()));
  alpha_property_ = new FloatProperty(
    "Alpha", 0.5f,
    "The amount of transparency to apply to the grid lines.",
    this, SLOT(updateColor()));
  alpha_property_->setMin(0.0f);
  alpha_property_->setMax(1.0f);
  alpha_property_->hide();

  plane_property_ = new EnumProperty(
    "Plane", "XY",
    "The plane to draw the grid along.",
    this, SLOT(updatePlane()));
  plane_property_->addOption("XY", XY);
  plane_property_->addOption("XZ", XZ);
  plane_property_->addOption("YZ", YZ);
  plane_property_->hide();

}

CustomGridDisplay::~CustomGridDisplay() = default;

void CustomGridDisplay::onInitialize()
{
  QColor color = color_property_->getColor();
  color.setAlphaF(alpha_property_->getFloat() );

  frame_property_->setFrameManager(context_->getFrameManager());
  grid_ = std::make_unique<Grid>(
    scene_manager_, scene_node_,
    Grid::Lines,
    10,
    cell_size_property_->getFloat(),
    0.03f,
    qtToOgre(color));

  grid_->getSceneNode()->setVisible(false);
  updatePlane();
}

void CustomGridDisplay::update(float dt, float ros_dt)
{
  Q_UNUSED(dt);
  Q_UNUSED(ros_dt);
  QString qframe = frame_property_->getFrame();
  std::string frame = qframe.toStdString();

  if (updateFrame(frame)) {
    setTransformOk();
    grid_->getSceneNode()->setVisible(true);
  } else {
    setMissingTransformToFixedFrame(qframe.toStdString());
    grid_->getSceneNode()->setVisible(false);
  }
}

void CustomGridDisplay::setStatus(StatusProperty::Level level,const QString & name,const QString & text)
{
  // do nothing
}

void CustomGridDisplay::updateColor()
{
  QColor color = color_property_->getColor();
  color.setAlphaF(alpha_property_->getFloat());
  grid_->setColor(qtToOgre(color));
  context_->queueRender();
}

void CustomGridDisplay::updateCellSize()
{
  grid_->setCellLength(cell_size_property_->getFloat());
  context_->queueRender();
}


void CustomGridDisplay::updatePlane()
{
  Ogre::Quaternion orient;
  switch ( (Plane) plane_property_->getOptionInt()) {
    case XZ:
      orient = Ogre::Quaternion(1, 0, 0, 0);
      break;
    case YZ:
      orient =
        Ogre::Quaternion(Ogre::Vector3(0, -1, 0), Ogre::Vector3(0, 0, 1), Ogre::Vector3(1, 0, 0));
      break;
    case XY:
    default:
      orient =
        Ogre::Quaternion(Ogre::Vector3(1, 0, 0), Ogre::Vector3(0, 0, -1), Ogre::Vector3(0, 1, 0));
      break;
  }
  grid_->getSceneNode()->setOrientation(orient);

  context_->queueRender();
}

}  // namespace displays
}  // namespace rviz_custom_plugins

#include <pluginlib/class_list_macros.hpp>  // NOLINT
PLUGINLIB_EXPORT_CLASS(rviz_custom_plugins::displays::CustomGridDisplay, rviz_common::Display)
